/*
	AUTHOR:
	Qiang Zhao, email: qiangzhao@tju.edu.cn
	Copyright (C) 2015 Tianjin University
	School of Computer Software
	School of Computer Science and Technology

	LICENSE:
	SPHORB is distributed under the GNU General Public License.  For information on
	commercial licensing, please contact the authors at the contact address below.

	REFERENCE:
	@article{zhao-SPHORB,
	author   = {Qiang Zhao and Wei Feng and Liang Wan and Jiawan Zhang},
	title    = {SPHORB: A Fast and Robust Binary Feature on the Sphere},
	journal  = {International Journal of Computer Vision},
	year     = {2015},
	volume   = {113},
	number   = {2},
	pages    = {143-159},
	}
*/

#include <algorithm>
#include <iterator>
#include <string>
#include "include/SPHORB.h"
#include "include/pfm.h"
#include "include/detector.h"
#include <iostream>
#define MAX_PATH 256

namespace cv
{
	std::vector<float *> geoinfos;
	std::vector<Mat> maskes;
	std::vector<vector<float *>> imgInfos;

	const int cells[] = {256, 204, 162, 128, 102, 80, 64};
	int levels;

	// load the precomputed information
	static void initSORB(std::string rootPath = "")
	{
		levels = sizeof(cells) / sizeof(cells[0]);
		for (int i = 0; i < levels; i++)
		{
			// geodesic grid coordinate with different resolution
			// char fileName[MAX_PATH];
			string fileName = rootPath + "Data/geoinfo" + std::to_string(cells[i]) + ".pfm";
			// sprintf(fileName, rootPath+"Data/geoinfo%d.pfm", cells[i]);
			// std::cerr<<"pfm file name: "<<fileName<<std::endl;
			float *geoinfo = new float[(cells[i] + 1) * (2 * cells[i] + 1) * 3];
			// read_pfm(fileName, geoinfo);
			read_pfm(fileName.c_str(), geoinfo);
			geoinfos.push_back(geoinfo);

			// look up table for fast image convertion from spherical image to geodesic grid
			vector<float *> partInfos;
			for (int j = 0; j < 5; j++)
			{
				// sprintf(fileName, rootPath+"Data/imginfo%d_%d.pfm", cells[i], j);
				fileName = rootPath + "Data/imginfo" + std::to_string(cells[i]) + "_" + std::to_string(j) + ".pfm";
				int num = (2 * cells[i] + 1) * (cells[i] + 1) * 4;
				num = static_cast<int>(ceil(num / 3.0)) * 3;
				std::cerr<<"Data/imginfo"<<std::to_string(cells[i])<<"_"<<std::to_string(j)<<".pfm: "<<num<<std::endl;
				float *partinfo = new float[num];
				// read_pfm(fileName, partinfo);
				read_pfm(fileName.c_str(), partinfo);

				std::cerr<<partinfo[0]<<"  ,  "<<partinfo[1]<<std::endl;

				partInfos.push_back(partinfo);
			}
			imgInfos.push_back(partInfos);

			// the mask image
			// sprintf(fileName, rootPath+"Data/mask%d.bmp", cells[i]);
			fileName = rootPath + "Data/mask" + std::to_string(cells[i]) + ".bmp";
			// Mat mask = imread(fileName, 0);
			Mat mask = imread(fileName.c_str(), 0);
			// cv::imshow("Mask",mask);
			// cv::waitKey(0);
			maskes.push_back(mask);
		}
	}

	static void uninitSORB()
	{	
		std::cerr<<"Uninit SORB"<<std::endl;
		for (size_t i = 0; i < geoinfos.size(); i++)
		{
			if (geoinfos[i] != NULL)
			{
				delete[] geoinfos[i];
				geoinfos[i] = NULL;
			}
		}
		for (size_t i = 0; i < imgInfos.size(); i++)
		{
			for (size_t j = 0; j < imgInfos[i].size(); j++)
			{
				if (imgInfos[i][j] != NULL)
				{
					delete[] imgInfos[i][j];
					imgInfos[i][j] = NULL;
				}
			}
		}

		geoinfos.clear();
		imgInfos.clear();
		maskes.clear();
	}

	// split spherical image to the storage grid
	static void splitSphere2(const Mat &im, Mat &oim, int idx, const float *imgInfo)
	{
		for (int y = 0; y < oim.rows; y++)
		{
			for (int x = 0; x < oim.cols; x++)
			{	
				// std::cerr<<"y: "<<y<<",x: "<<x<<std::endl;
				float lx = imgInfo[(x + y * oim.cols) * 4];
				float ly = imgInfo[(x + y * oim.cols) * 4 + 1];
				float wh = imgInfo[(x + y * oim.cols) * 4 + 2];
				float wv = imgInfo[(x + y * oim.cols) * 4 + 3];
				// std::cerr<<"lx: "<<lx<<std::endl;
				// std::cerr<<"ly: "<<ly<<std::endl;
				// std::cerr<<"wh: "<<wh<<std::endl;
				// std::cerr<<"wv: "<<wv<<std::endl;
				int ix = static_cast<int>(lx);
				int iy = static_cast<int>(ly);
				// std::cerr<<"ix: "<<ix<<std::endl;
				// std::cerr<<"iy: "<<iy<<std::endl;
				uchar v1 = im.at<uchar>(iy, ix);
				uchar v2 = im.at<uchar>(iy, (ix + 1) % im.cols);
				uchar v3 = im.at<uchar>(iy + 1, ix);
				uchar v4 = im.at<uchar>(iy + 1, (ix + 1) % im.cols);
				// std::cerr<<"v1: "<<v1<<std::endl;
				// std::cerr<<"v2: "<<v2<<std::endl;
				// std::cerr<<"v3: "<<v3<<std::endl;
				// std::cerr<<"v4: "<<v4<<std::endl;
				
				float v12 = v1 * wh + v2 * (1 - wh);
				float v34 = v3 * wh + v4 * (1 - wh);
				oim.at<uchar>(y, x) = uchar(v12 * wv + v34 * (1 - wv));
			}
		}
	}

	// extend the storage grid from top and right boundary
	static void extendTopRight(Mat &newPart1, const Mat &part2, int edge)
	{
		int h = part2.rows;
		int w = part2.cols;

		int r, c;

		r = edge;
		for (c = edge - 1; c < h + edge - 1; c++)
		{

			int c0 = c - edge + 1;
			int rn = c0;
			int cn = 0;
			for (int i = 1; i <= edge; i++)
			{
				rn--;
				cn++;
				if (rn >= 0)
					newPart1.at<uchar>(r - i, c) = part2.at<uchar>(rn, cn);
				else
					break;
			}
		}

		for (c = h + edge - 1; c < w + edge - 1; c++)
		{
			int c0 = c - edge + 1;
			int rn = h - 1;
			int cn = c0 - h + 1;
			for (int i = 1; i <= edge; i++)
			{
				rn--;
				if (rn + cn >= h - 1)
					newPart1.at<uchar>(r - i, c) = part2.at<uchar>(rn, cn);
				else
					break;
			}
		}

		c = w + edge - 2;
		for (r = edge; r < h + edge; r++)
		{
			int r0 = r - edge;
			int rn = h - 1;
			int cn = r0 + h - 1;
			for (int i = 1; i <= edge; i++)
			{
				rn--;
				cn++;
				if (cn < 2 * h - 1)
					newPart1.at<uchar>(r, c + i) = part2.at<uchar>(rn, cn);
				else
					break;
			}
		}
	}

	// extend the storage grid from left and bottom boundary
	static void extendBottomLeft(Mat &newPart1, const Mat &part2, int edge)
	{
		int h = part2.rows;
		int w = part2.cols;

		int r, c;

		c = edge - 1;
		for (r = edge; r < h + edge; r++)
		{
			int r0 = r - edge;
			int c0 = c - edge + 1;
			int rn = c0;
			int cn = r0;
			for (int i = 1; i <= edge - 1; i++)
			{
				rn++;
				cn--;
				if (cn >= 0)
					newPart1.at<uchar>(r, c - i) = part2.at<uchar>(rn, cn);
				else
					break;
			}
		}

		r = h + edge - 1;
		for (c = edge - 1; c < h + edge - 2; c++)
		{
			int c0 = c - edge + 1;
			int rn = 0;
			int cn = c0 + h - 1;
			for (int i = 1; i <= edge - 1; i++)
			{
				rn++;
				if (rn + cn <= 2 * h - 2)
					newPart1.at<uchar>(r + i, c) = part2.at<uchar>(rn, cn);
				else
					break;
			}
		}

		for (c = h + edge - 2; c < w + edge - 1; c++)
		{
			int c0 = c - edge + 1;
			int rn = c0 - h + 1;
			int cn = 2 * h - 2;
			for (int i = 1; i <= edge - 1; i++)
			{
				rn++;
				cn--;
				if (rn < h)
					newPart1.at<uchar>(r + i, c) = part2.at<uchar>(rn, cn);
				else
					break;
			}
		}
	}

	// extend the storage grid
	static void extendEdge(Mat &part0, Mat &part1, Mat &part2, Mat &part3, Mat &part4, int edge)
	{
		int height = part0.rows + edge * 2 - 1;
		int width = part0.cols + edge * 2 - 1;

		// extend the image size
		Mat _part0 = Mat::zeros(Size(width, height), part0.type());
		Mat _part1 = Mat::zeros(Size(width, height), part1.type());
		Mat _part2 = Mat::zeros(Size(width, height), part2.type());
		Mat _part3 = Mat::zeros(Size(width, height), part3.type());
		Mat _part4 = Mat::zeros(Size(width, height), part4.type());

		// copy the source data
		for (int y = edge; y < height - edge + 1; y++)
		{
			for (int x = edge - 1; x < width - edge; x++)
			{
				_part0.at<uchar>(y, x) = part0.at<uchar>(y - edge, x - edge + 1);
				_part1.at<uchar>(y, x) = part1.at<uchar>(y - edge, x - edge + 1);
				_part2.at<uchar>(y, x) = part2.at<uchar>(y - edge, x - edge + 1);
				_part3.at<uchar>(y, x) = part3.at<uchar>(y - edge, x - edge + 1);
				_part4.at<uchar>(y, x) = part4.at<uchar>(y - edge, x - edge + 1);
			}
		}

		// extend the edges
		extendTopRight(_part0, part1, edge);
		extendTopRight(_part1, part2, edge);
		extendTopRight(_part2, part3, edge);
		extendTopRight(_part3, part4, edge);
		extendTopRight(_part4, part0, edge);

		extendBottomLeft(_part1, part0, edge);
		extendBottomLeft(_part2, part1, edge);
		extendBottomLeft(_part3, part2, edge);
		extendBottomLeft(_part4, part3, edge);
		extendBottomLeft(_part0, part4, edge);

		// copy the extend image
		part0 = _part0;
		part1 = _part1;
		part2 = _part2;
		part3 = _part3;
		part4 = _part4;
	}

	// the angle between the x-axis of local coordinate and the south pole
	static float inherentAngle(const float *center, const float *axisx)
	{
		float t1 = -center[0] * axisx[0] - center[1] * axisx[1] - center[2] * axisx[2] + 1.0f;
		float x1 = axisx[0] + center[0] * t1;
		float y1 = axisx[1] + center[1] * t1;
		float z1 = axisx[2] + center[2] * t1;

		float t2 = center[2] + 1.0f;
		float x2 = center[0] * t2;
		float y2 = center[1] * t2;
		float z2 = -1 + center[2] * t2;

		x1 -= center[0];
		y1 -= center[1];
		z1 -= center[2];

		x2 -= center[0];
		y2 -= center[1];
		z2 -= center[2];

		float tmp = (x1 * x2 + y1 * y2 + z1 * z2) / (sqrt((x1 * x1 + y1 * y1 + z1 * z1) * (x2 * x2 + y2 * y2 + z2 * z2)));
		if (tmp > 1.0f)
			tmp = 1.0f;
		if (tmp < -1.0f)
			tmp = -1.0f;
		float radian = acos(tmp);

		if (center[0] * axisx[1] - axisx[0] * center[1] < 0)
			return (float)(-radian * (180.f / CV_PI));
		else
			return (float)(radian * (180.f / CV_PI));
	}

	static float IC_Angle(const Mat &img, const int half_k, Point2f pt, const float *geoinfo)
	{
		float m_01 = 0, m_10 = 0;

		int step = (int)img.step1();
		const uchar *center = &img.at<uchar>(cvRound(pt.y), cvRound(pt.x));

		float tmp = sqrt(3.0f) * 0.5f;

		for (int y = -half_k; y <= half_k; y++)
		{
			int xmax, xmin;
			if (y < 0)
			{
				xmin = -y - half_k;
				xmax = half_k;
			}
			else
			{
				xmin = -half_k;
				xmax = half_k - y;
			}

			for (int x = xmin; x <= xmax; x++)
			{
				float euclidX = x + y * 0.5f;
				float euclidY = tmp * y;

				m_10 += euclidX * center[x + y * step];
				m_01 += euclidY * center[x + y * step];
			}
		}

		return fastAtan2((float)m_01, (float)m_10);

		// int x = pt.x - half_k - 2;
		// int y = pt.y - half_k - 3;
		// return fastAtan2((float)m_01, (float)m_10) - inherentAngle(geoinfo+(x+y*513)*3, geoinfo+(x+1+y*513)*3);
	}

	static void computeOrientation(const Mat &image, vector<KeyPoint> &kps, int halfPatchSize, const float *geoinfo)
	{
		for (size_t i = 0; i < kps.size(); i++)
		{
			kps[i].angle = IC_Angle(image, halfPatchSize, kps[i].pt, geoinfo);
		}
	}

	static void computeOrbDescriptor(const KeyPoint &kpt, const Mat &img, const Point *pattern, sphbyte *desc, int dsize)
	{
		float angle = kpt.angle;
		angle *= (float)(CV_PI / 180.f);
		float a = (float)cos(angle), b = (float)sin(angle);
		float c = sqrt(3.0f);
		float d = b * c / 3;
		b = a - d;
		a = a + d;
		c = 2 * d;

		int step = (int)img.step;
		const uchar *center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));

		// Transform the sampling patterns from rectangle structure to the Euclidean space,
		// after rotating the patterns, transform them to the original space.
#define GET_VALUE(idx)                                               \
	center[cvRound(pattern[idx].y * a + pattern[idx].x * c) * step + \
		   cvRound(pattern[idx].x * b - pattern[idx].y * c)]

		for (int i = 0; i < dsize; ++i, pattern += 16)
		{
			int t0, t1, val;
			t0 = GET_VALUE(0);
			t1 = GET_VALUE(1);
			val = t0 < t1;
			t0 = GET_VALUE(2);
			t1 = GET_VALUE(3);
			val |= (t0 < t1) << 1;
			t0 = GET_VALUE(4);
			t1 = GET_VALUE(5);
			val |= (t0 < t1) << 2;
			t0 = GET_VALUE(6);
			t1 = GET_VALUE(7);
			val |= (t0 < t1) << 3;
			t0 = GET_VALUE(8);
			t1 = GET_VALUE(9);
			val |= (t0 < t1) << 4;
			t0 = GET_VALUE(10);
			t1 = GET_VALUE(11);
			val |= (t0 < t1) << 5;
			t0 = GET_VALUE(12);
			t1 = GET_VALUE(13);
			val |= (t0 < t1) << 6;
			t0 = GET_VALUE(14);
			t1 = GET_VALUE(15);
			val |= (t0 < t1) << 7;

			desc[i] = (uchar)val;
		}

#undef GET_VALUE
	}

	static void computeDescriptors(const Mat &image, vector<KeyPoint> &keypoints, Mat &descriptors,
								   const vector<Point> &pattern, int dsize, int WTA_K)
	{

		descriptors = Mat::zeros(keypoints.size(), dsize, CV_8UC1);

		for (size_t i = 0; i < keypoints.size(); i++)
		{
			computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i), dsize);
		}
	}
	// map the keypoint of each level of the five part of the storage grid to the original spherical image
	static void mappingKeypoint(const Mat &img, vector<cv::KeyPoint> &kps, int edge, const float *geoinfo, int level)
	{

		float scale = float(cells[0]) / float(cells[level]);
		int pWidth = cells[level] * 2 + 1;

		float pcos[5] = {cos(0.0), cos(2 * CV_PI / 5), cos(4 * CV_PI / 5), cos(6 * CV_PI / 5), cos(8 * CV_PI / 5)};
		float psin[5] = {sin(0.0), sin(2 * CV_PI / 5), sin(4 * CV_PI / 5), sin(6 * CV_PI / 5), sin(8 * CV_PI / 5)};
		float c = CV_PI / img.rows;
		for (size_t i = 0; i < kps.size(); i++)
		{
			int _x = static_cast<int>(kps[i].pt.x);
			int _y = static_cast<int>(kps[i].pt.y);

			int x = static_cast<int>(kps[i].pt.x - edge + 1);
			int y = static_cast<int>(kps[i].pt.y - edge);

			float sX3D = geoinfo[(x + y * pWidth) * 3];
			float sY3D = geoinfo[(x + y * pWidth) * 3 + 1];
			float sZ3D = geoinfo[(x + y * pWidth) * 3 + 2];

			float dX3D = pcos[kps[i].class_id] * sX3D - psin[kps[i].class_id] * sY3D;
			float dY3D = pcos[kps[i].class_id] * sY3D + psin[kps[i].class_id] * sX3D;
			float dZ3D = sZ3D;

			float theta = acos(dZ3D);
			float phi = atan2(dY3D, dX3D) + CV_PI;

			float panoX = phi / c;
			float panoY = theta / c;

			kps[i].size = 31.0f * scale;
			kps[i].pt.x = panoX * scale;
			kps[i].pt.y = panoY * scale;
			kps[i].class_id = -1;
			kps[i].octave = level;
		}
	}

	// the gaussian used to smooth the storage grid
	static double kernel[] = {0, 0, 0, 0.007615469730253, 0.012684563109382, 0.012684563109382, 0.007615469730253,
							  0, 0, 0.012684563109382, 0.027267400652990, 0.035191124791545, 0.027267400652990, 0.012684563109382,
							  0, 0.012684563109382, 0.035191124791545, 0.058615431367971, 0.058615431367971, 0.035191124791545, 0.012684563109382,
							  0.007615469730253, 0.027267400652990, 0.058615431367971, 0.075648683430860, 0.058615431367971, 0.027267400652990, 0.007615469730253,
							  0.012684563109382, 0.035191124791545, 0.058615431367971, 0.058615431367971, 0.035191124791545, 0.012684563109382, 0,
							  0.012684563109382, 0.027267400652990, 0.035191124791545, 0.027267400652990, 0.012684563109382, 0, 0,
							  0.007615469730253, 0.012684563109382, 0.012684563109382, 0.007615469730253, 0, 0, 0};

	// the sampling pattern
	static int bit_pattern[256 * 4] =
		{
			0, 13, -15, 14,	  /*mean (0.000196695), correlation (0)*/
			-15, 1, -14, 15,  /*mean (0.00235352), correlation (0.0867396)*/
			15, -12, -4, -7,  /*mean (0.00343412), correlation (0.0907921)*/
			9, -15, 2, -1,	  /*mean (0.00463572), correlation (0.0883058)*/
			15, -1, 5, 3,	  /*mean (0.0126926), correlation (0.0987857)*/
			10, -8, 13, 2,	  /*mean (0.0138597), correlation (0.0987887)*/
			-7, 4, -15, 6,	  /*mean (0.0160338), correlation (0.0988597)*/
			4, 11, -5, 13,	  /*mean (0.0174904), correlation (0.0941022)*/
			1, -14, 4, -10,	  /*mean (0.0179875), correlation (0.098813)*/
			7, -4, -9, 11,	  /*mean (0.0201184), correlation (0.0996794)*/
			-5, 2, -1, 4,	  /*mean (0.0202611), correlation (0.0999593)*/
			10, -11, 15, -8,  /*mean (0.021882), correlation (0.0984285)*/
			9, 2, 9, 6,		  /*mean (0.0244927), correlation (0.0994798)*/
			-2, 8, -4, 14,	  /*mean (0.027259), correlation (0.0940522)*/
			-4, -10, -5, -5,  /*mean (0.0273411), correlation (0.0986831)*/
			-1, -9, -9, -6,	  /*mean (0.0275832), correlation (0.0981432)*/
			-12, -1, -15, 4,  /*mean (0.0298567), correlation (0.0972344)*/
			15, -6, 12, -3,	  /*mean (0.0306563), correlation (0.0982352)*/
			-5, -5, -8, 0,	  /*mean (0.0332757), correlation (0.0995229)*/
			6, 1, 2, 2,		  /*mean (0.0345897), correlation (0.0925487)*/
			2, -8, 4, -7,	  /*mean (0.0381859), correlation (0.0958004)*/
			-3, 5, -3, 8,	  /*mean (0.0386657), correlation (0.0910819)*/
			8, 6, 6, 9,		  /*mean (0.0458667), correlation (0.0944299)*/
			-10, -3, -13, -2, /*mean (0.0459575), correlation (0.0948076)*/
			15, -14, 13, -12, /*mean (0.0489183), correlation (0.0961879)*/
			-13, 12, -15, 13, /*mean (0.0500119), correlation (0.0896462)*/
			5, 10, 3, 12,	  /*mean (0.0518014), correlation (0.0956848)*/
			-2, 14, -2, 15,	  /*mean (0.0703832), correlation (0.0863209)*/
			-13, 6, -14, 7,	  /*mean (0.0727475), correlation (0.0964942)*/
			-13, -2, -14, -1, /*mean (0.0860172), correlation (0.0972335)*/
			-1, 2, -8, 4,	  /*mean (0.0284995), correlation (0.128915)*/
			10, -7, 6, -5,	  /*mean (0.032502), correlation (0.123528)*/
			1, -3, -1, 1,	  /*mean (0.0335177), correlation (0.128973)*/
			2, 1, 2, 5,		  /*mean (0.0335869), correlation (0.128039)*/
			8, -12, 11, -11,  /*mean (0.0341488), correlation (0.129793)*/
			6, -13, 3, -12,	  /*mean (0.0355104), correlation (0.129379)*/
			-12, 13, -10, 15, /*mean (0.0367466), correlation (0.128868)*/
			3, -14, 0, -13,	  /*mean (0.0413023), correlation (0.129571)*/
			-5, 0, -6, 3,	  /*mean (0.0419853), correlation (0.125114)*/
			-6, -2, -3, -2,	  /*mean (0.0509023), correlation (0.127468)*/
			0, -15, 0, -14,	  /*mean (0.070206), correlation (0.10119)*/
			5, 9, 5, 10,	  /*mean (0.0703054), correlation (0.115902)*/
			1, -5, 0, -4,	  /*mean (0.0784703), correlation (0.124267)*/
			-15, 3, -13, 3,	  /*mean (0.08163), correlation (0.11899)*/
			3, 6, 4, 6,		  /*mean (0.10536), correlation (0.122715)*/
			-9, -1, -8, -1,	  /*mean (0.11161), correlation (0.117768)*/
			4, -3, 5, -3,	  /*mean (0.11652), correlation (0.128808)*/
			-11, 9, -10, 9,	  /*mean (0.127084), correlation (0.128628)*/
			15, -13, 11, -5,  /*mean (0.0197813), correlation (0.166973)*/
			13, -5, 4, 7,	  /*mean (0.020408), correlation (0.16229)*/
			-1, -12, -2, 12,  /*mean (0.0221456), correlation (0.165757)*/
			-9, 7, -14, 12,	  /*mean (0.0226513), correlation (0.149167)*/
			12, 3, 0, 10,	  /*mean (0.0229366), correlation (0.164526)*/
			-8, 3, -11, 9,	  /*mean (0.0255689), correlation (0.168939)*/
			-1, -13, -5, -8,  /*mean (0.0271812), correlation (0.167755)*/
			-11, 10, -10, 13, /*mean (0.0295369), correlation (0.156488)*/
			-1, 8, -5, 9,	  /*mean (0.0315122), correlation (0.168131)*/
			7, -3, 10, -2,	  /*mean (0.0327181), correlation (0.168398)*/
			-11, -4, -10, -2, /*mean (0.0330985), correlation (0.158571)*/
			4, 2, -2, 3,	  /*mean (0.0336431), correlation (0.163403)*/
			7, -15, 10, -14,  /*mean (0.0341056), correlation (0.165908)*/
			14, -3, 11, -1,	  /*mean (0.0364397), correlation (0.159289)*/
			10, -15, 7, -13,  /*mean (0.0418945), correlation (0.168564)*/
			0, -4, -3, -3,	  /*mean (0.0433079), correlation (0.156452)*/
			-14, 5, -12, 6,	  /*mean (0.0465065), correlation (0.166389)*/
			-7, -8, -11, -4,  /*mean (0.0471116), correlation (0.161426)*/
			3, -11, 2, -9,	  /*mean (0.049839), correlation (0.167447)*/
			6, -2, 5, 0,	  /*mean (0.0509239), correlation (0.15398)*/
			4, -8, 2, -6,	  /*mean (0.053902), correlation (0.164873)*/
			-14, 9, -13, 10,  /*mean (0.0549912), correlation (0.163488)*/
			1, 4, -1, 6,	  /*mean (0.0588771), correlation (0.160916)*/
			7, 0, 7, 1,		  /*mean (0.0700893), correlation (0.166851)*/
			3, -9, 3, -8,	  /*mean (0.0721899), correlation (0.165257)*/
			15, -7, 15, -6,	  /*mean (0.0724406), correlation (0.158128)*/
			13, 0, 15, 0,	  /*mean (0.0758682), correlation (0.161292)*/
			-1, 8, 0, 8,	  /*mean (0.113045), correlation (0.16224)*/
			0, -2, 1, -2,	  /*mean (0.113477), correlation (0.167667)*/
			14, -7, 15, -7,	  /*mean (0.118954), correlation (0.147736)*/
			14, -15, 15, -15, /*mean (0.119381), correlation (0.168126)*/
			-10, 15, -9, 15,  /*mean (0.125251), correlation (0.162249)*/
			-13, 3, 7, 6,	  /*mean (0.00479999), correlation (0.219219)*/
			14, -4, 1, -2,	  /*mean (0.00824922), correlation (0.21777)*/
			-10, -5, 4, 3,	  /*mean (0.00968426), correlation (0.209761)*/
			15, -15, -13, 8,  /*mean (0.010955), correlation (0.219427)*/
			-2, 9, -10, 15,	  /*mean (0.0132329), correlation (0.209071)*/
			15, -10, 4, 1,	  /*mean (0.0133064), correlation (0.217042)*/
			-7, -7, -15, 10,  /*mean (0.0166562), correlation (0.218543)*/
			3, -9, 10, -3,	  /*mean (0.0195003), correlation (0.219571)*/
			8, -11, -15, 0,	  /*mean (0.0202956), correlation (0.212531)*/
			3, 7, 7, 8,		  /*mean (0.0210477), correlation (0.219609)*/
			15, -2, 0, 15,	  /*mean (0.0218949), correlation (0.216145)*/
			-9, 14, -15, 15,  /*mean (0.0225303), correlation (0.198854)*/
			-1, 4, 4, 9,	  /*mean (0.022673), correlation (0.216455)*/
			-12, 7, -3, 10,	  /*mean (0.0232176), correlation (0.219485)*/
			7, -6, 1, -5,	  /*mean (0.0241555), correlation (0.216268)*/
			-9, -2, -8, 2,	  /*mean (0.0242852), correlation (0.218637)*/
			15, -11, 10, -9,  /*mean (0.0246223), correlation (0.214)*/
			4, -12, -6, -2,	  /*mean (0.0260055), correlation (0.212964)*/
			-3, 11, 1, 14,	  /*mean (0.0261957), correlation (0.209467)*/
			1, -6, -9, 5,	  /*mean (0.0271509), correlation (0.216454)*/
			-1, -7, -1, -3,	  /*mean (0.0273022), correlation (0.219368)*/
			0, -1, -5, 7,	  /*mean (0.0289015), correlation (0.218391)*/
			8, -10, 8, -7,	  /*mean (0.0296319), correlation (0.214665)*/
			7, -7, 6, -2,	  /*mean (0.0300728), correlation (0.213401)*/
			4, 5, 2, 11,	  /*mean (0.0308984), correlation (0.208114)*/
			-4, -8, -1, -7,	  /*mean (0.0317975), correlation (0.213124)*/
			-3, 7, -7, 8,	  /*mean (0.0326101), correlation (0.202289)*/
			-7, 8, -10, 10,	  /*mean (0.0341272), correlation (0.211545)*/
			1, -13, -3, -12,  /*mean (0.0359383), correlation (0.200318)*/
			-4, -3, -11, 2,	  /*mean (0.0367855), correlation (0.219147)*/
			-11, 4, -9, 5,	  /*mean (0.0373517), correlation (0.212568)*/
			7, 1, 8, 3,		  /*mean (0.0381038), correlation (0.211246)*/
			-4, -2, -3, 0,	  /*mean (0.0403428), correlation (0.219019)*/
			14, -1, 14, 1,	  /*mean (0.0415919), correlation (0.217099)*/
			-6, -9, -4, -8,	  /*mean (0.0417778), correlation (0.205248)*/
			-14, 3, -13, 5,	  /*mean (0.0421149), correlation (0.214165)*/
			-15, 12, -15, 14, /*mean (0.0423613), correlation (0.21214)*/
			-7, -4, -11, -3,  /*mean (0.0431004), correlation (0.214436)*/
			8, -12, 6, -10,	  /*mean (0.0434246), correlation (0.200937)*/
			-6, 9, -6, 11,	  /*mean (0.0478334), correlation (0.206306)*/
			14, -4, 15, -3,	  /*mean (0.053647), correlation (0.188974)*/
			3, 6, 1, 7,		  /*mean (0.0573556), correlation (0.212569)*/
			9, -14, 9, -13,	  /*mean (0.0688487), correlation (0.199587)*/
			-14, 7, -14, 8,	  /*mean (0.0689525), correlation (0.214022)*/
			-6, 15, -3, 15,	  /*mean (0.0692032), correlation (0.217487)*/
			15, -15, 15, -14, /*mean (0.071628), correlation (0.181987)*/
			11, 4, 10, 5,	  /*mean (0.0722159), correlation (0.207965)*/
			2, 13, 1, 14,	  /*mean (0.0743209), correlation (0.216403)*/
			14, 1, 13, 2,	  /*mean (0.0767025), correlation (0.211112)*/
			8, 5, 10, 5,	  /*mean (0.0767759), correlation (0.187129)*/
			7, 5, 6, 6,		  /*mean (0.077005), correlation (0.218884)*/
			4, 3, 3, 4,		  /*mean (0.0780165), correlation (0.219462)*/
			2, 12, 3, 12,	  /*mean (0.113101), correlation (0.204923)*/
			10, -2, 11, -2,	  /*mean (0.116071), correlation (0.206195)*/
			-15, 0, -14, 0,	  /*mean (0.117125), correlation (0.194557)*/
			-15, 15, -14, 15, /*mean (0.118236), correlation (0.213365)*/
			-1, -7, 0, -7,	  /*mean (0.118854), correlation (0.203522)*/
			6, -10, 7, -10,	  /*mean (0.120851), correlation (0.218447)*/
			-5, 12, -4, 12,	  /*mean (0.121279), correlation (0.20892)*/
			4, -15, -15, 14,  /*mean (0.00336927), correlation (0.273755)*/
			6, -14, 8, 5,	  /*mean (0.00488642), correlation (0.279447)*/
			15, -6, -15, 13,  /*mean (0.00646842), correlation (0.255713)*/
			-14, -1, -1, 1,	  /*mean (0.00707355), correlation (0.283177)*/
			-2, 5, -15, 11,	  /*mean (0.00939897), correlation (0.275681)*/
			12, -9, -2, -1,	  /*mean (0.00999546), correlation (0.281584)*/
			14, 1, -15, 9,	  /*mean (0.0104018), correlation (0.281006)*/
			12, -14, -9, 15,  /*mean (0.0105098), correlation (0.278655)*/
			-7, 6, 8, 7,	  /*mean (0.0107605), correlation (0.280916)*/
			15, -12, 7, 8,	  /*mean (0.0115775), correlation (0.281633)*/
			-2, -13, 13, -1,  /*mean (0.0119449), correlation (0.283968)*/
			-10, -3, -7, 14,  /*mean (0.0126407), correlation (0.285558)*/
			14, -14, 3, -4,	  /*mean (0.0138424), correlation (0.283279)*/
			10, -11, 0, 3,	  /*mean (0.0141881), correlation (0.283786)*/
			13, -7, -10, -2,  /*mean (0.0142357), correlation (0.284256)*/
			3, -13, 6, -6,	  /*mean (0.0145685), correlation (0.284786)*/
			2, -15, -2, -6,	  /*mean (0.015031), correlation (0.283388)*/
			-3, -11, 6, -8,	  /*mean (0.0151909), correlation (0.285178)*/
			12, 0, -9, 14,	  /*mean (0.0154589), correlation (0.28538)*/
			14, -2, -6, 0,	  /*mean (0.0154589), correlation (0.284792)*/
			-10, 1, -4, 3,	  /*mean (0.015688), correlation (0.27994)*/
			-4, 2, -12, 13,	  /*mean (0.0157788), correlation (0.284405)*/
			-4, -4, 4, -1,	  /*mean (0.0162499), correlation (0.283384)*/
			1, -10, 2, 2,	  /*mean (0.0162715), correlation (0.282837)*/
			-9, 2, -2, 15,	  /*mean (0.0165741), correlation (0.284352)*/
			3, -4, 14, 0,	  /*mean (0.0168853), correlation (0.285193)*/
			-7, 10, -12, 14,  /*mean (0.0170582), correlation (0.280953)*/
			-13, 6, -10, 12,  /*mean (0.0173046), correlation (0.282108)*/
			7, -2, -15, 5,	  /*mean (0.0177498), correlation (0.277614)*/
			1, 0, 7, 4,		  /*mean (0.0181431), correlation (0.282959)*/
			-1, -14, -15, 6,  /*mean (0.0181604), correlation (0.281862)*/
			-5, 11, -11, 12,  /*mean (0.0186315), correlation (0.282597)*/
			15, -9, -3, 13,	  /*mean (0.0188174), correlation (0.285063)*/
			15, -7, 8, -4,	  /*mean (0.019764), correlation (0.283551)*/
			-14, 2, -4, 8,	  /*mean (0.0204902), correlation (0.284785)*/
			11, -14, 13, -10, /*mean (0.0206371), correlation (0.26634)*/
			-7, -8, -3, 5,	  /*mean (0.0209526), correlation (0.279196)*/
			-6, 8, 0, 12,	  /*mean (0.0210564), correlation (0.279815)*/
			-6, 1, -5, 6,	  /*mean (0.0210996), correlation (0.274191)*/
			15, -13, 7, -11,  /*mean (0.0212898), correlation (0.284267)*/
			-10, 9, -15, 10,  /*mean (0.0213935), correlation (0.264998)*/
			12, -7, 9, 0,	  /*mean (0.0215102), correlation (0.281889)*/
			14, -15, 9, -8,	  /*mean (0.0222234), correlation (0.285447)*/
			-6, -6, 5, 10,	  /*mean (0.0222407), correlation (0.282781)*/
			11, -3, -3, 5,	  /*mean (0.0225779), correlation (0.277698)*/
			6, -15, 3, -11,	  /*mean (0.0226427), correlation (0.276495)*/
			-4, -1, 0, 9,	  /*mean (0.0228415), correlation (0.28444)*/
			4, -3, -7, -1,	  /*mean (0.0232348), correlation (0.283978)*/
			5, -8, 1, 6,	  /*mean (0.0232392), correlation (0.279056)*/
			11, -2, 10, 4,	  /*mean (0.0233127), correlation (0.282934)*/
			-9, 0, -15, 8,	  /*mean (0.0241555), correlation (0.284957)*/
			4, 7, -5, 15,	  /*mean (0.0241555), correlation (0.268863)*/
			8, -8, -9, 3,	  /*mean (0.0241771), correlation (0.285399)*/
			2, -6, -10, -4,	  /*mean (0.0243068), correlation (0.283939)*/
			5, -9, -4, -5,	  /*mean (0.0244019), correlation (0.2797)*/
			6, 1, -5, 12,	  /*mean (0.0248644), correlation (0.285081)*/
			7, -3, 0, 8,	  /*mean (0.0255387), correlation (0.28087)*/
			-4, 4, 0, 5,	  /*mean (0.0260314), correlation (0.278163)*/
			-7, -6, -5, -2,	  /*mean (0.0262519), correlation (0.283694)*/
			0, -10, -15, 3,	  /*mean (0.0263253), correlation (0.278577)*/
			15, -9, 11, -7,	  /*mean (0.0264377), correlation (0.270823)*/
			10, -10, 6, -8,	  /*mean (0.026844), correlation (0.273929)*/
			4, -2, 2, 13,	  /*mean (0.0271077), correlation (0.285601)*/
			1, -6, 6, -5,	  /*mean (0.0272374), correlation (0.278473)*/
			2, -15, 15, -15,  /*mean (0.027756), correlation (0.285406)*/
			6, -5, -2, 2,	  /*mean (0.0279592), correlation (0.28443)*/
			-4, 1, -11, 7,	  /*mean (0.0286681), correlation (0.282063)*/
			0, -5, 1, -2,	  /*mean (0.0287199), correlation (0.274318)*/
			9, 2, 4, 4,		  /*mean (0.0289058), correlation (0.277918)*/
			9, 0, 13, 1,	  /*mean (0.0291219), correlation (0.281146)*/
			-6, 11, -7, 15,	  /*mean (0.0297443), correlation (0.284571)*/
			-1, 0, 2, 1,	  /*mean (0.0298049), correlation (0.271109)*/
			-9, -1, -15, 2,	  /*mean (0.0313393), correlation (0.285131)*/
			-12, -2, -10, 0,  /*mean (0.0316073), correlation (0.283623)*/
			-1, 6, -1, 10,	  /*mean (0.0316678), correlation (0.283354)*/
			15, -10, 15, -7,  /*mean (0.0320006), correlation (0.273356)*/
			-2, 4, -7, 10,	  /*mean (0.0322297), correlation (0.270098)*/
			1, 3, -5, 5,	  /*mean (0.0327268), correlation (0.28388)*/
			9, -8, 11, -6,	  /*mean (0.0328175), correlation (0.273843)*/
			-7, 6, -6, 9,	  /*mean (0.0330553), correlation (0.284424)*/
			4, -11, -1, -10,  /*mean (0.0330639), correlation (0.276539)*/
			-12, 7, -15, 8,	  /*mean (0.0338376), correlation (0.282607)*/
			5, 9, 0, 11,	  /*mean (0.0339197), correlation (0.275682)*/
			-9, -6, -14, 1,	  /*mean (0.0345421), correlation (0.271055)*/
			-2, 12, -5, 14,	  /*mean (0.0347194), correlation (0.280668)*/
			-9, 11, -6, 13,	  /*mean (0.0353677), correlation (0.262672)*/
			10, -5, 12, -3,	  /*mean (0.035506), correlation (0.281089)*/
			-8, 3, -6, 4,	  /*mean (0.0360377), correlation (0.284495)*/
			2, 0, -3, 1,	  /*mean (0.0364915), correlation (0.285368)*/
			-1, 9, 1, 10,	  /*mean (0.036552), correlation (0.278575)*/
			6, -1, 3, 1,	  /*mean (0.0367898), correlation (0.285137)*/
			-2, -13, 0, -11,  /*mean (0.0371658), correlation (0.279776)*/
			14, -6, 15, -4,	  /*mean (0.0376197), correlation (0.24281)*/
			8, -4, 5, -1,	  /*mean (0.0376413), correlation (0.279717)*/
			-7, 6, -10, 8,	  /*mean (0.0380217), correlation (0.276929)*/
			11, -3, 12, -1,	  /*mean (0.03867), correlation (0.283306)*/
			4, 3, 5, 5,		  /*mean (0.0387435), correlation (0.284163)*/
			8, 7, 4, 8,		  /*mean (0.0387478), correlation (0.283709)*/
			-9, 14, -5, 15,	  /*mean (0.0388429), correlation (0.279198)*/
			13, 2, 10, 4,	  /*mean (0.0392492), correlation (0.281706)*/
			-10, 3, -13, 4,	  /*mean (0.0394307), correlation (0.280171)*/
			-14, 0, -13, 2,	  /*mean (0.0394567), correlation (0.270968)*/
			-1, -2, -6, 1,	  /*mean (0.0413758), correlation (0.278449)*/
			1, 10, -1, 13,	  /*mean (0.0416568), correlation (0.281767)*/
			-11, 5, -10, 7,	  /*mean (0.0421668), correlation (0.277155)*/
			-1, -4, -3, -1,	  /*mean (0.0424434), correlation (0.281096)*/
			1, -10, -2, -9,	  /*mean (0.0427071), correlation (0.273452)*/
			0, 6, -3, 8,	  /*mean (0.0438655), correlation (0.273137)*/
			-7, 2, -10, 3,	  /*mean (0.0441292), correlation (0.28543)*/
			-3, -6, -6, -3,	  /*mean (0.0472931), correlation (0.282788)*/
			15, -12, 13, -10, /*mean (0.0473407), correlation (0.27601)*/
			6, -10, 4, -9,	  /*mean (0.051395), correlation (0.27438)*/
			1, 0, 0, 2,		  /*mean (0.052735), correlation (0.276666)*/
			0, -14, -1, -12,  /*mean (0.0538113), correlation (0.273652)*/
			4, -12, 2, -11,	  /*mean (0.053889), correlation (0.28349)*/
			6, 4, 4, 5,		  /*mean (0.0542867), correlation (0.27425)*/
			-3, -11, -5, -10, /*mean (0.0548529), correlation (0.272518)*/
	};

	SPHORB::SPHORB(std::string rootPath_, int _nfeatures, int _nlevels, int b) : nfeatures(_nfeatures), barrier(b), rootPath(rootPath_)
	{

		initSORB(rootPath_);
		nlevels = min(_nlevels, levels);
		std::cerr<<"nlevels: "<<nlevels<<std::endl;
		std::cerr<<"_nlevels: "<<_nlevels<<std::endl;
	}

	SPHORB::~SPHORB()
	{
		// uninitSORB();
	}
	
	void SPHORB::releaseMemory()
	{
		uninitSORB();
	}
	int SPHORB::descriptorSize() const
	{
		return kBytes;
	}

	int SPHORB::descriptorType() const
	{
		return CV_8U;
	}

	void SPHORB::operator()(InputArray _image, InputArray _mask, vector<KeyPoint> &_keypoints,
							OutputArray _descriptors, bool useProvidedKeypoints) const
	{
		bool do_keypoints = !useProvidedKeypoints;
		bool do_descriptors = _descriptors.needed();

		if ((!do_keypoints && !do_descriptors) || _image.empty())
		{
			std::cerr << "do_keypoints: " << (int)do_keypoints << std::endl;
			std::cerr << "do_descriptors: " << (int)do_descriptors << std::endl;
			std::cerr << "Image empty: " << (int)_image.empty() << std::endl;
			return;
		}
		// std::cerr << "Detect" << std::endl;
		_keypoints.clear();
		Mat temp = _image.getMat();
		Mat descriptors;
		if (temp.type() != CV_8UC1)
			cvtColor(_image, temp, CV_BGR2GRAY);
		// std::cerr << "After converting" << std::endl;
		// compute how many features should be detected on every scale space level
		vector<int> nfeaturesPerLevel(nlevels);
		float factor = (float)(1.0 / pow(2.0, 1 / 3.0));
		float ndesiredFeaturesPerScale = nfeatures * (1 - factor) / (1 - (float)pow((double)factor, (double)nlevels));

		int sumFeatures = 0;
		for (int level = 0; level < nlevels - 1; level++)
		{
			nfeaturesPerLevel[level] = cvRound(ndesiredFeaturesPerScale);
			sumFeatures += nfeaturesPerLevel[level];
			ndesiredFeaturesPerScale *= factor;
		}
		nfeaturesPerLevel[nlevels - 1] = std::max(nfeatures - sumFeatures, 0);

		// Gaussian kernel for hexagonal grid
		Mat matKernel(7, 7, CV_64F, kernel);

		// sampling pattern
		const int npoints = 512;
		vector<Point> pattern;
		const Point *pattern0 = (const Point *)bit_pattern;

		std::copy(pattern0, pattern0 + 512, std::back_inserter(pattern));

		// std::cerr << "Detect" << std::endl;
		// detect and describe the features on every level
		for (int l = 0; l < nlevels; l++)
		{
			// resize the spherical image
			// std::cerr << "Level: " << l << std::endl;
			Size sz(cells[l] * 5, cells[l] * 5 / 2);
			// std::cerr<<"sz:"<<sz<<std::endl;
			Mat image(sz, temp.type());
			resize(temp, image, sz, 0, 0, CV_INTER_AREA);

			// split the spherical image to five parts
			Mat subImg[5];
			// std::cerr << "split" << std::endl;
			for (int i = 0; i < 5; i++)
			{
				subImg[i].create(Size(2 * cells[l] + 1, cells[l] + 1), image.type());
				// std::cerr << "split i = " << i << std::endl;
				// std::cerr<<"Image type: "<<  image.type()<<std::endl;
				// std::cerr<<"CV_8UC1 "<<  CV_8UC1<<std::endl;
				// std::cerr<<imgInfos[l][i][0]<<"  ,  "<<imgInfos[l][i][1]<<std::endl;
				splitSphere2(image, subImg[i], i, imgInfos[l][i]);
			}
			// std::cerr << "extendEdge" << std::endl;
			// extend each part for boundary pixels
			extendEdge(subImg[0], subImg[1], subImg[2], subImg[3], subImg[4], SFAST_EDGE + SPHORB_EDGE);

			// the key points on each level
			vector<KeyPoint> levelKeyPoints;

			for (int i = 0; i < 5; i++)
			{
				xy *corners;
				int *score;
				int cor_num;
				vector<KeyPoint> partKeyPoints;

				// detect the key points and do the non-max suppression
				corners = sfast_corner_detect(&subImg[i].at<uchar>(0, 0), &maskes[l].at<uchar>(0, 0),
											  maskes[l].cols, (int)maskes[l].step, maskes[l].rows, barrier, &cor_num);
				score = sfastScore(&subImg[i].at<uchar>(0, 0), (int)subImg[i].step, corners, cor_num, barrier);
				// std::cerr<<"Corner num: "<<cor_num<<std::endl;
				sfastNonmaxSuppression(corners, score, cor_num, partKeyPoints, i);

				levelKeyPoints.insert(levelKeyPoints.end(), partKeyPoints.begin(), partKeyPoints.end());

				delete[] corners;
				delete[] score;
			}

			if (levelKeyPoints.size() > nfeaturesPerLevel[l])
				KeyPointsFilter::retainBest(levelKeyPoints, nfeaturesPerLevel[l]);

			// std::cerr<<"detect point size per level:" << levelKeyPoints.size()<<std::endl;
			// compute the orientation
			for (size_t i = 0; i < levelKeyPoints.size(); i++)
				levelKeyPoints[i].angle = IC_Angle(subImg[levelKeyPoints[i].class_id], SPHORB_EDGE, levelKeyPoints[i].pt, geoinfos[l]);

			// filter the image
			for (int i = 0; i < 5; i++)
				filter2D(subImg[i], subImg[i], -1, matKernel);

			Mat tDesc = Mat::zeros(levelKeyPoints.size(), kBytes, CV_8UC1);

			for (size_t i = 0; i < levelKeyPoints.size(); i++)
			{
				computeOrbDescriptor(levelKeyPoints[i], subImg[levelKeyPoints[i].class_id], &pattern[0], tDesc.ptr((int)i), kBytes);
			}

			descriptors.push_back(tDesc);

			mappingKeypoint(image, levelKeyPoints, SFAST_EDGE + SPHORB_EDGE, geoinfos[l], l);

			_keypoints.insert(_keypoints.end(), levelKeyPoints.begin(), levelKeyPoints.end());
		}

		descriptors.copyTo(_descriptors);
	}

	void SPHORB::operator()(InputArray image, InputArray mask, vector<KeyPoint> &keypoints) const
	{
		(*this)(image, mask, keypoints, noArray(), false);
	}

	void SPHORB::detectImpl(const Mat &image, vector<KeyPoint> &keypoints, const Mat &mask) const
	{
		(*this)(image, mask, keypoints, noArray(), false);
	}

	void SPHORB::computeImpl(const Mat &image, vector<KeyPoint> &keypoints, Mat &descriptors) const
	{
		(*this)(image, Mat(), keypoints, descriptors, true);
	}
}
